#ifndef ANTI_SHAPE_HPP
#define ANTI_SHAPE_HPP

#include <stdio.h>
#include <stdlib.h>
#include <string>
#include <eigen3/Eigen/Dense>
#include <opencv2/core/eigen.hpp>
#include <opencv2/opencv.hpp>
#include <chrono>
#include <vector>
#include <queue>
#include <fstream> 
#include "efficientGoodFeaturesToTrack.hpp"
#include <json/json.h>
#include <cmath>
#include <cassert>
#include <thread>


struct tranParam{
    float x,y,a;
};

struct AntiShapeParam{   
    float cropRatio = 0.1;
    float alpha = 0.95;
    float alphaMin = 1.0;
    float alphaMax = 0;
    float beta = 2.6;
    float gamma = 0.95;
    float damping = 0.95;
    float innerPaddingRatio = 0.0;
    int useSmooth = 0;
    int lookaheadNo = 0;
    float dx = 1.0;
    float dy = 1.0;
    int dealtWidth = 0;
    int dealtHeight = 0;
    int frameWidth = 0;
    int frameHeight = 0;

    AntiShapeParam();

    void load(std::string filePath);
};


class AntiShape{
private:
    AntiShapeParam config;
    std::queue<cv::Mat> queue;
    Eigen::MatrixXd preP, preV, preDeltaP, preDeltaV, prePtV2P;
    cv::Mat imgPrev;
    Eigen::MatrixXd ScaleMat;
    std::deque<Eigen::MatrixXd>  PBuffer, VBuffer, deltaPBuffer, deltaVBuffer;
    std::deque<cv::Mat> imageBuffer;
    int outerDist;
    Eigen::MatrixXd cropRoi, innerRoi, outerRoi;
    int frameCropRoiX1, frameCropRoiX2, frameCropRoiY1, frameCropRoiY2;

public:
    AntiShape(std::string configFile);
    void put(cv::Mat& img);
    void get_xya(const Eigen::MatrixXd& rotM, tranParam& param);
    void slerp_like(Eigen::MatrixXd& rotM, const Eigen::MatrixXd& rotM0, 
                    const Eigen::MatrixXd& rotM1, float alpha);
    void cal_v_cam_velocity(Eigen::MatrixXd& currDeltaV, const Eigen::MatrixXd& preDeltaP, 
                                                         const Eigen::MatrixXd& curPtV2P);
    bool boundary_detection(int& uncompensatedDist, const Eigen::MatrixXd& curPtV2P);
    bool update_filter(Eigen::MatrixXd& currDeltaV,bool isOutOfBoundary, const Eigen::MatrixXd& curPtV2P, int uncompensatedDist);
    void updateRotationMatrix(Eigen::MatrixXd& rotM);
    void smooth(Eigen::MatrixXd& smoothedRotM, const std::deque<Eigen::MatrixXd>& trajMatrixs);
    void getMovement(Eigen::MatrixXd& rotM, const cv::Mat& imgCurrGray, const cv::Mat& imgPrevGray);
    void get(cv::Mat& frameAffined, cv::Mat& frame);
};

#endif //ANTI_SHAPE_HPP